// Copyright 2020 Makani Technologies LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "gs/state_command/gs02_state.h"

#include <stdint.h>
#include <string.h>

#include "avionics/common/avionics_messages.h"
#include "avionics/common/cvt_avionics_messages.h"
#include "avionics/common/pack_avionics_messages.h"
#include "avionics/common/safety_codes.h"
#include "avionics/linux/aio.h"
#include "avionics/network/aio_labels.h"
#include "gs/state_command/state_command.h"

// Different from other state-command nodes since gs02 iterates by actuator
// not by aio_label. Mimics autogenerated aio_labels code since the gs actuator
// enum does not get built into the same autogen functions as the aio nodes.
const char *GsActuatorToString(GroundStationActuator gs_actuator) {
  switch (gs_actuator) {
    case kGroundStationActuatorAzimuth:
      return "Azimuth";
    case kGroundStationActuatorDetwist:
      return "Detwist";
    case kGroundStationActuatorLevelwind:
      return "Levelwind";
    case kGroundStationActuatorWinch:
      return "Winch";
    default:
    case kGroundStationActuatorForceSigned:
    case kNumGroundStationActuators:
      assert(false);
      return "<Unknown>";
  }
}

StateCommandBase::LabelsMap Gs02StateCommand::GetLabels(void) const {
  LabelsMap labels;
  for (int32_t i = 0; i < kNumGroundStationActuators; ++i) {
    labels[i] = GsActuatorToString(static_cast<GroundStationActuator>(i));
  }
  return labels;
}

bool Gs02StateCommand::GetActuatorState(int32_t index, ActuatorState *state) {
  // Different from other node types, PLC has only one AIO node with multiple
  // actuators. For PLC, the int32_t index references the four actuators.
  AioNode node = kAioNodePlcGs02;
  GroundStationStatusMessage status;
  if (CvtGetGroundStationStatusMessage(node, &status, nullptr, nullptr)) {
    *state = status.actuator_state[index];
    return true;
  }
  return false;
}

void Gs02StateCommand::SendActuatorStateCommand(
    ActuatorStateCommand command) {
  GroundStationSetStateMessage m;
  memset(&m, 0, sizeof(m));
  m.actuator_mask = static_cast<uint8_t>(GetPendingTargets());
  m.state_command = command;
  if (command == kActuatorStateCommandArm) {
    m.arming_signal = GROUND_STATION_ARMING_SIGNAL;
  } else if (command == kActuatorStateCommandDisarm) {
    m.arming_signal = GROUND_STATION_DISARM_SIGNAL;
  }
  AIO_SEND_PACKED(kMessageTypeGroundStationSetState,
                  PackGroundStationSetStateMessage,
                  PACK_GROUNDSTATIONSETSTATEMESSAGE_SIZE, &m);
}
